#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped
from std_msgs.msg import Header

class SimpleForcePublisher(Node):
    def __init__(self):
        # 初始化节点
        super().__init__('simple_force_publisher')

        # 创建发布者
        self.publisher_ = self.create_publisher(TwistStamped, '/force_data', 10)

        # 创建定时器，每0.1秒发布一次数据
        self.timer = self.create_timer(0.1, self.publish_force_data)

        # 添加时间计数器和状态标志
        self.start_time = self.get_clock().now()
        self.force_state = True  # True表示0.3，False表示0

        self.get_logger().info('Simple force publisher started')

    def publish_force_data(self):
        # 创建TwistStamped消息对象
        twist_force = TwistStamped()
        twist_force.header = Header()
        twist_force.header.stamp = self.get_clock().now().to_msg()
        twist_force.header.frame_id = 'simple_force_sensor'

        # 计算当前时间与开始时间的差值
        current_time = self.get_clock().now()
        time_diff = (current_time - self.start_time).nanoseconds / 1e9  # 转换为秒

        # 每5秒切换一次状态
        if time_diff >= 3.0:
            self.force_state = not self.force_state
            self.start_time = current_time

        # 根据状态设置力数据
        force_value = -2.0 if self.force_state else 0.0
        twist_force.twist.linear.x = 0.0
        twist_force.twist.linear.y = 0.0
        twist_force.twist.linear.z = force_value

        # 发布消息
        self.publisher_.publish(twist_force)

        # 打印发布的消息
        self.get_logger().info(
            f"Force data: X={twist_force.twist.linear.x}, "
            f"Y={twist_force.twist.linear.y}, "
            f"Z={twist_force.twist.linear.z}"
        )

def main(args=None):
    # 初始化ROS2
    rclpy.init(args=args)

    # 创建简单力发布节点
    force_publisher = SimpleForcePublisher()

    try:
        # 保持节点运行
        rclpy.spin(force_publisher)
    except KeyboardInterrupt:
        pass

    # 销毁节点并关闭ROS2
    force_publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
# #!/usr/bin/env python3
# # -*- coding: utf-8 -*-

# import rclpy
# from rclpy.node import Node
# from geometry_msgs.msg import TwistStamped
# from std_msgs.msg import Header

# class SimpleForcePublisher(Node):
#     def __init__(self):
#         # 初始化节点
#         super().__init__('simple_force_publisher')

#         # 创建发布者
#         self.publisher_ = self.create_publisher(TwistStamped, '/force_data', 10)

#         # 创建定时器，每0.1秒发布一次数据
#         self.timer = self.create_timer(0.1, self.publish_force_data)

#         self.get_logger().info('Simple force publisher started')

#     def publish_force_data(self):
#         # 创建TwistStamped消息对象
#         twist_force = TwistStamped()
#         twist_force.header = Header()
#         twist_force.header.stamp = self.get_clock().now().to_msg()
#         twist_force.header.frame_id = 'simple_force_sensor'

#         # 设置固定的力数据
#         twist_force.twist.linear.x = 0.0
#         twist_force.twist.linear.y = 0.0
#         twist_force.twist.linear.z = 0.1

#         # 发布消息
#         self.publisher_.publish(twist_force)

#         # 打印发布的消息
#         self.get_logger().info(
#             f"Force data: X={twist_force.twist.linear.x}, "
#             f"Y={twist_force.twist.linear.y}, "
#             f"Z={twist_force.twist.linear.z}"
#         )

# def main(args=None):
#     # 初始化ROS2
#     rclpy.init(args=args)

#     # 创建简单力发布节点
#     force_publisher = SimpleForcePublisher()

#     try:
#         # 保持节点运行
#         rclpy.spin(force_publisher)
#     except KeyboardInterrupt:
#         pass

#     # 销毁节点并关闭ROS2
#     force_publisher.destroy_node()
#     rclpy.shutdown()

# if __name__ == '__main__':
#     main()